#include "qtode.h"

#include <QTimer>
#include <QList>

#include<QtGui>

#include <ode/ode.h>
//#include <gui/glwidget.h>

#include <math.h>

const int nBodies = 40;
const float stageSize = 250.0;
#define drand48()  ((double) (((double) rand()) / ((double) RAND_MAX)))
#define TIME_STEP       0.01

class QtOdePrivate{
	public:	
		QtOdePrivate() {
			err_reduct = REAL( 0.5 );
			cf_mixing = REAL( 0.001 );

		}
		~QtOdePrivate() {}


		dWorld	dyn_world;
		dBody	dyn_bodies[nBodies];
		dReal	bodies_sides[nBodies][3];
		dSpaceID coll_space_id;
		dJointID plane2d_joint_ids[nBodies];
		dJointGroup coll_contacts;
		dReal  cf_mixing;// = 1 / TIME_STEP * K_SPRING + K_DAMP;
		dReal  err_reduct;// = TIME_STEP * K_SPRING * cf_mixing;

		QList<QGraphicsItem*> items;
		QTimer *timer;


};

//const int QtOdePrivate::nBodies = 40;

QtOde::QtOde(QObject *parent) :
	QGraphicsScene(parent), O_PRIVATE_CREATE(QtOde)
{
	O_D(QtOde)
	dInitODE2(0);
	dWorldSetERP(d->dyn_world.id(), d->err_reduct);

	dWorldSetCFM(d->dyn_world.id (), d->cf_mixing);
	d->dyn_world.setGravity(0, 0.0, -1.0);

	d->coll_space_id = dSimpleSpaceCreate(0);
	// dynamic bodies
	for (int b = 0; b < nBodies; b ++)
	{
		int     l = (int) (1 + sqrt ((double) nBodies));
		dReal  x = dReal( (0.5 + (b / l)) / l * stageSize );
		dReal  y = dReal( (0.5 + (b % l)) / l * stageSize );
		dReal  z = REAL(1.0) + REAL( 0.1 ) * (dReal)drand48();

//		d->bodies_sides[b][0] = dReal( 5 * (0.2 + 0.7*drand48()) / sqrt((double)nBodies) );
//		d->bodies_sides[b][1] = dReal( 5 * (0.2 + 0.7*drand48()) / sqrt((double)nBodies) );
		d->bodies_sides[b][0] = dReal( 5 * (10 + 30*drand48()) / sqrt((double)nBodies) );
		d->bodies_sides[b][1] = dReal( 5 * (5 + 30*drand48()) / sqrt((double)nBodies) );
		d->bodies_sides[b][2] = z;

		d->dyn_bodies[b].create(d->dyn_world);
		d->dyn_bodies[b].setPosition (x, y, z/2);
		d->dyn_bodies[b].setData ((void*) (size_t)b);

		dBodySetLinearVel(d->dyn_bodies[b].id (),
						   dReal( 3 * (drand48 () - 0.5) ),
						   dReal( 3 * (drand48 () - 0.5) ), 0);

		dMass m;
		m.setBox (1, d->bodies_sides[b][0], d->bodies_sides[b][1], d->bodies_sides[b][2]);
		m.adjust (REAL(0.1) * d->bodies_sides[b][0] * d->bodies_sides[b][1]);
		d->dyn_bodies[b].setMass(&m);

		d->plane2d_joint_ids[b] = dJointCreatePlane2D (d->dyn_world.id (), 0);
		dJointAttach (d->plane2d_joint_ids[b], d->dyn_bodies[b].id (), 0);
	}

	dJointSetPlane2DXParam (d->plane2d_joint_ids[0], dParamFMax, 10);
	dJointSetPlane2DYParam (d->plane2d_joint_ids[0], dParamFMax, 10);


	// collision geoms and joints
	dCreatePlane(d->coll_space_id, 1, 0, 0, 0);
	dCreatePlane(d->coll_space_id,-1, 0, 0, -stageSize);
	dCreatePlane(d->coll_space_id, 0, 1, 0, 0);
	dCreatePlane(d->coll_space_id, 0, -1, 0, -stageSize);

	for (int b = 0; b < nBodies; b ++)
	{
		dGeomID coll_box_id;
		coll_box_id = dCreateBox (d->coll_space_id,
								  d->bodies_sides[b][0], d->bodies_sides[b][1], d->bodies_sides[b][2]);
		dGeomSetBody(coll_box_id, d->dyn_bodies[b].id());
	}


	d->coll_contacts.create ();



	dAllocateODEDataForThread(dAllocateMaskAll);

    static float    xyz[3] = { 0.5f*stageSize, 0.5f*stageSize, 0.65f*stageSize};
    static float    hpr[3] = { 90.0f, -90.0f, 0 };

    //setSceneRect(0, 0, xyz[0], xyz[1]);
    for (int b = 0; b < nBodies; b ++)
           {
//               if (b == 0)
//               dsSetColor (1.0, 0.5, 1.0);
//               else
//               dsSetColor (0, 0.5, 1.0);
//   #ifdef dDOUBLE
//               dsDrawBoxD (dyn_bodies[b].getPosition(), dyn_bodies[b].getRotation(), bodies_sides[b]);
//   #else
               //dsDrawBox (dyn_bodies[b].getPosition(), dyn_bodies[b].getRotation(), bodies_sides[b]);
               QGraphicsItem* i = addRect(0, 0, 30, 30);
               d->items.append(i);

           }
   // dsSetViewpoint (xyz, hpr);
    d->timer = new QTimer(this);
    connect(d->timer, SIGNAL(timeout()), this, SLOT(updateScene()));

}

void QtOde::start()
{
    O_D(QtOde);
    d->timer->start(25);

}


static void cb_near_collision (void *data, dGeomID o1, dGeomID o2)
/********************************************************************/
{
    dBodyID     b1 = dGeomGetBody(o1);
    dBodyID     b2 = dGeomGetBody(o2);

    // exit without doing anything if the two bodies are static
    if (b1 == 0 && b2 == 0)
        return;

    // exit without doing anything if the two bodies are connected by a joint
    if (b1 && b2 && dAreConnected(b1, b2))
    {
        /* MTRAP; */
        return;
    }

    dContact contact;
    contact.surface.mode = 0;
    contact.surface.mu = 0; // frictionless

    QtOdePrivate *d = (QtOdePrivate*)data;
    if (dCollide(o1, o2, 1, &contact.geom, sizeof (dContactGeom)))
    {
        dJointID c = dJointCreateContact (d->dyn_world.id(),
                        d->coll_contacts.id (), &contact);
        dJointAttach(c, b1, b2);
    }
}


static void track_to_pos (dBody &body, dJointID joint_id, dReal target_x, dReal target_y)
/************************************************************************/
{
    dReal  curr_x = body.getPosition()[0];
    dReal  curr_y = body.getPosition()[1];

    dJointSetPlane2DXParam (joint_id, dParamVel, 1 * (target_x - curr_x));
    dJointSetPlane2DYParam (joint_id, dParamVel, 1 * (target_y - curr_y));
}

void QtOde::updateScene()
{
    O_D(QtOde);
    static dReal angle = 0;

    angle += REAL( 0.11 );

    track_to_pos (d->dyn_bodies[0], d->plane2d_joint_ids[0],
        dReal( stageSize/2 + stageSize/2.0 * cos (angle) ),
        dReal( stageSize/2 + stageSize/2.0 * sin (angle) ));

    double   f0 = 0.101;
    for (int b = 0; b < nBodies; b ++)
    {
        double p = 1 + b / (double) nBodies;
        double q = 2 - b / (double) nBodies;
        d->dyn_bodies[b].addForce (f0 * cos (p*angle), f0 * sin (q*angle), 0);
    }
     d->dyn_bodies[0].addTorque (0, 0.0, 0.1);

    const int n = 10;
    for (int i = 0; i < n; i ++)
    {
        dSpaceCollide (d->coll_space_id, d, &cb_near_collision);
        d->dyn_world.step (dReal(TIME_STEP/n));
        d->coll_contacts.empty();
    }

    for (int b = 0; b < nBodies; b ++)
   {

        QGraphicsRectItem* i = (QGraphicsRectItem*)d->items[b];
        if (b == 0){
            i->setPen(QPen(Qt::blue));
            i->setBrush(QBrush(Qt::green));
        }else{
            i->setPen(QPen(Qt::red));
            i->setBrush(QBrush(Qt::yellow));
        }
        //i->setPos();
        const dReal* pos = d->dyn_bodies[b].getPosition();
        dReal x = pos[0];
        dReal y = pos[1];
        dReal z = pos[2];

        dReal* size = d->bodies_sides[b];
        dReal w = size[0];
        dReal h = size[1];
       //i->setPos(x, y );
        //i->setRect(QRectF(x * 100, y* 100, w* 100, h*100));
        i->setRect(QRectF(x , y, w, h));

        const dReal* rots = d->dyn_bodies[b].getRotation();
        QMatrix matrix;
        //matrix.translate(50, 50);
        //matrix.rotate(rots[0] * 100);
       // matrix.scale(0.5, 1.0);


       // i->setMatrix(matrix);
        i->setRotation(rots[0]);
      // dsDrawBox (d->dyn_bodies[b].getPosition(), d->dyn_bodies[b].getRotation(), d->bodies_sides[b]);


   }
}

QtOde::~QtOde()
{
	dCloseODE();
	O_PRIVATE_DELETE;
}
